#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>

#include <pcl_ros/transforms.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_eigen/tf2_eigen.hpp>

#include <geometry_msgs/msg/detail/vector3__struct.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <nav_msgs/msg/path.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/float32.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include <pcl/filters/voxel_grid.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2/transform_datatypes.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>


// #include "points_downsampler.h"

#define MAX_MEASUREMENT_RANGE 120.0

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_points_pub;
rclcpp::Node::SharedPtr _ros2_node;
// Leaf size of VoxelGrid filter.
static double voxel_leaf_size = 2.0;

static int _output_log = 0;
static std::ofstream ofs;
static std::string filename;

static std::string POINTS_TOPIC;

static pcl::PointCloud<pcl::PointXYZ> removePointsByRange(pcl::PointCloud<pcl::PointXYZ> scan, double min_range, double max_range)
{
  pcl::PointCloud<pcl::PointXYZ> narrowed_scan;
  narrowed_scan.header = scan.header;

  if( min_range>=max_range ) {
    RCLCPP_INFO(_ros2_node->get_logger(), "min_range>=max_range @(%lf, %lf)", min_range, max_range );
    return scan;
  }

  double square_min_range = min_range * min_range;
  double square_max_range = max_range * max_range;

  for(pcl::PointCloud<pcl::PointXYZ>::const_iterator iter = scan.begin(); iter != scan.end(); ++iter)
  {
    const pcl::PointXYZ &p = *iter;
    double square_distance = p.x * p.x + p.y * p.y;

    if(square_min_range <= square_distance && square_distance <= square_max_range){
      narrowed_scan.points.push_back(p);
    }
  }

  return narrowed_scan;
}

static void scan_callback(const sensor_msgs::msg::PointCloud2::ConstPtr& input)
{
  pcl::PointCloud<pcl::PointXYZ> scan;
  pcl::fromROSMsg(*input, scan);
  scan = removePointsByRange(scan, 0, MAX_MEASUREMENT_RANGE);

  pcl::PointCloud<pcl::PointXYZ>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(scan));
  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());

  sensor_msgs::msg::PointCloud2 filtered_msg;

  // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL)
  if (voxel_leaf_size >= 0.1)
  {
    // Downsampling the velodyne scan using VoxelGrid filter
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
    voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
    voxel_grid_filter.setInputCloud(scan_ptr);
    voxel_grid_filter.filter(*filtered_scan_ptr);
    pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);
  }
  else
  {
    pcl::toROSMsg(*scan_ptr, filtered_msg);
  }

  filtered_msg.header = input->header;
  filtered_points_pub->publish(filtered_msg);

}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
	_ros2_node = rclcpp::Node::make_shared("voxel_grid_filter");

  _ros2_node->declare_parameter<std::string>("points_topic", "");
	_ros2_node->get_parameter("points_topic", POINTS_TOPIC);
	RCLCPP_INFO(_ros2_node->get_logger(), "points_topic %s", POINTS_TOPIC.c_str());

  _ros2_node->declare_parameter<int>("_output_log", 0);
	_ros2_node->get_parameter("_output_log", _output_log);
	RCLCPP_INFO(_ros2_node->get_logger(), "_output_log %d", _output_log);

  _ros2_node->declare_parameter<double>("leaf_size", 0.2);
	_ros2_node->get_parameter("leaf_size", voxel_leaf_size);
	RCLCPP_INFO(_ros2_node->get_logger(), "leaf_size %f", voxel_leaf_size);

  if(_output_log == 1){
	  char buffer[80];
	  std::time_t now = std::time(NULL);
	  std::tm *pnow = std::localtime(&now);
	  std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow);
	  filename = "voxel_grid_filter_" + std::string(buffer) + ".csv";
	  ofs.open(filename.c_str(), std::ios::app);
  }

  rclcpp::QoS qos(rclcpp::KeepLast{10});

  // Publishers
  filtered_points_pub = _ros2_node->create_publisher<sensor_msgs::msg::PointCloud2>("/filtered_points", qos);

  // Subscribers
  auto scan_sub = _ros2_node->create_subscription<sensor_msgs::msg::PointCloud2>(POINTS_TOPIC, qos, scan_callback);

  rclcpp::spin(_ros2_node);
	rclcpp::shutdown();

  return 0;
}
